#ifndef HEIGHT_KF_H
#define HEIGHT_KF_H

#include <stdio.h>
#include <ros/ros.h>
#include <boost/thread.hpp>
#include <cyphy_kbd/KeyCode.h>

#include <time.h>

#include <vector>
#include <numeric>
#include <functional>
#include <iostream>
#include <iterator>
#include "Kalman.h"
#include <Eigen/Geometry>

#include "sensor_msgs/Imu.h"
#include "std_msgs/Float32MultiArray.h"
#include "std_msgs/Float32.h"
#include <cyphy_kbd/KeyCode.h>

#define PI 3.141592653589793238462643383279
#define deg2rad(x) (x*0.0174532925f)
#define rad2deg(x) (x*57.2957795f)
#define g 9.81

#define KEYCODE_SPACEBAR 0x20 //to start
#define KEYCODE_ESC 0x1B //to finish
// for rectangle path
#define KEYCODE_q 0x71 // 'q'
#define KEYCODE_w 0x77 // 'w'
#define KEYCODE_s 0x73 // 's'
#define KEYCODE_a 0x61 // 'a'

// for triangle path
#define KEYCODE_d 0x64 // 'd'
#define KEYCODE_f 0x66 // 'f'
#define KEYCODE_r 0x72 // 'r'
#define KEYCODE_e 0x65 // 'e'
#define KEYCODE_v 0x76 // 'v'
#define KEYCODE_t 0x74 // 't'
#define KEYCODE_9 0x39 // 't'
enum {RECEIVE_START,ON_AIR,ON_LANDING};


#define LOOP_HZ 70//1//100

using namespace std;

ros::Publisher  m_kf_pub;
ros::Subscriber HSubscriber; 
ros::Subscriber mkIMUSub;
ros::Subscriber kbdSubscriber;

sensor_msgs::Imu mk_Imu;
std_msgs::Float32MultiArray height_kf_out;
ros::Time current_time;
Kalman kalman;
double height;
Vector2f estimates;
double ts;
int state=100;

void HCallback(const std_msgs::Float32::ConstPtr& msg);
void mkIMU_Callback(const sensor_msgs::ImuConstPtr& imu_msg);
void kbdCallback (const cyphy_kbd::KeyCode& msg);

#ifdef KALMAN
Kalman kalman;
#endif


#endif

